#include "GpTreeCreature.h"

#include "FunctionNode.h"
#include "TerminalNode.h"

#include <deque>
namespace jcrada {

    void GpTreeCreature::evaluate( const FunctionNode& function ) {
        TerminalNode* a = dynamic_cast<TerminalNode*> (function.getChild(0));
        TerminalNode* b = dynamic_cast<TerminalNode*> (function.getChild(1));
        assert(a && b);
        switch (function.getType()) {
            case FunctionNode::FT_MOVE : move(a->getValue(), b->getValue());
                break;
            case FunctionNode::FT_SWAP : swap(a->getValue(), b->getValue());
                break;
            default:
                BDEBUG(TO_STRING(function.getType()));
                assert(0);
        }
    }

    void GpTreeCreature::move( int a, int b ) {
        IdNode* root = dynamic_cast<IdNode*> (getMorphology().getRoot());
        assert(root);
        IdNode* node_a = root->findNodeByIdUsingBFS(a);
        IdNode* node_b = root->findNodeByIdUsingBFS(b);

        bool do_swap = node_a->findNodeByIdUsingBFS(b) || node_b->findNodeByIdUsingBFS(a);
        if (do_swap) { //node_b \in node_a
            swap(*node_a, *node_b);
            return;
        }
        Node* parent_a = node_a->getParent();
        Node* parent_b = node_b->getParent();

        int position_a = parent_a->getChildPosition(node_a);
        int position_b = parent_b->getChildPosition(node_b);

        node_a->remove();
        node_b->remove();

        node_a->moveTo(parent_b, position_b);
        node_b->moveTo(parent_a, position_a);
    }

    void GpTreeCreature::swap( int a, int b ) {
        IdNode* root = dynamic_cast<IdNode*> (getMorphology().getRoot());
        assert(root);
        IdNode* node_a = root->findNodeByIdUsingBFS(a);
        IdNode* node_b = root->findNodeByIdUsingBFS(b);
        assert(node_a && node_b);
        int tmp_id = node_a->getId();
        node_a->setId(node_b->getId());
        node_b->setId(tmp_id);
    }

    void GpTreeCreature::swap( IdNode& a, IdNode& b ) {
        int tmp_id = a.getId();
        a.setId(b.getId());
        b.setId(tmp_id);
    }

    void GpTreeCreature::setup( Creature& creature ) {
        const IdNode* parent = dynamic_cast<const IdNode*> (getMorphology().getRoot());
        creature.getBodyPart(parent->getId()).setParentId(Creature::B_NONE);
        std::deque<const IdNode*> deque;
        deque.push_back(parent);
        const IdNode* child = NULL;
        int constraint = 0;
        while (deque.size() > 0) {
            parent = deque.front();
            for (int i = 0; i < parent->getArity(); ++i) {
                child = dynamic_cast<IdNode*> (parent->getChild(i));
                if (child) {
                    creature.getBodyPart(child->getId()).setParentId(parent->getId());
                    creature.getConstraint(constraint).setIdBodyA(parent->getId());
                    creature.getConstraint(constraint).setIdBodyB(child->getId());
                    ++constraint;
                    deque.push_back(child);
                }
            }
            deque.pop_front();
        }

    }
}
